import datetime
import time

from src.constants.delay_time import DelayTime
from src.driver.linear_motor_driver import LinearMotorDriver


class MotorAction:
    def __init__(self):
        # 日志
        self.motor = LinearMotorDriver()
        self.logger = self.motor.logger
        # 设置急停时的减速度
        # self.motor.set_point_emergency_stop_deceleration(1000)

    def get_zero_status(self):
        """
        获取回零标志
        0-回零未完成
        1-回零已经完成
        :return:
        """
        driver_status = self.motor.get_driver_status()
        zero_status = driver_status[-5]
        self.logger.info_log(f"motor zero status is: {zero_status}")
        return zero_status

    def get_motion_status(self):
        """
        获取电机运动状态
        0-电机停止状态
        1-电机正在运行
        注意：电机处于运行状态时，无法响应新的运动指令，只能响应停止指令
        :return:
        """
        driver_status = self.motor.get_driver_status()
        move_status = driver_status[-4]
        self.logger.info_log(f"motor move status is: {move_status}")
        return move_status

    def get_driver_position_complete_status(self) -> str:
        """
        闭环模式时电机定位完成标志
        0-定位未完成
        1-定位完成
        :return:
        """
        driver_status = self.motor.get_driver_status()
        position_complete_status = driver_status[-3]
        self.logger.info_log(f"driver position complete status is: {position_complete_status}")
        return position_complete_status

    def get_driver_alarm_status(self):
        """
        驱动器报警标志
        0-无报警
        1-发生了报警，请检查寄存器 REG_ALMCODE（地址 0）的状态
        :return:
        """
        driver_status = self.motor.get_driver_status()
        alarm_status = driver_status[-2]
        self.logger.info_log(f"driver alarm status is: {alarm_status}")
        return alarm_status

    def get_driver_enable_status(self):
        """
        获取驱动器使能标志
        0-驱动器未使能
        1-驱动器已经使能
        :return:
        """
        driver_status = self.motor.get_driver_status()
        driver_enable_status = driver_status[-1]
        self.logger.info_log(f"driver enable status is: {driver_enable_status}")
        return driver_enable_status

    def set_zero_position(self):
        """
        设置零点
        :return:
        """
        self.motor.confirm_zero_position()

    def zeroing(self):
        """
        启动回零
        :return:
        """
        # 启动回零
        # self.motor.start_to_zero()
        while True:
            current_absolute_position = self.get_current_absolute_position()
            self.logger.info_log(f"motor run to zero, current absolute position: {current_absolute_position}")

    def set_point_to_point_acceleration(self, point_acceleration: int):
        """
        设置点对点运动加速度，单位：(r/s^2)
        register: 70
        :return:
        """
        self.motor.set_point_io_acceleration(point_acceleration)

    def set_point_to_point_deceleration(self, point_deceleration: int):
        """
        设置点对点运动减速度(r/s^2)
        :return:
        """
        self.motor.set_point_io_deceleration(point_deceleration)

    def set_point_to_point_max_speed(self, max_speed: int):
        """
        设置点对点运动最大速度（RPM）
        :return:
        """
        self.motor.set_point_io_rotate_speed(max_speed)

    def set_point_to_point_trip(self, trip_pulse_count: int):
        """
        设置点对点运动行程，单位：个
        register: 73（行程低16位）、74（行程高16位）
        :param trip_pulse_count: 脉冲数
        :return:
        """
        pass

    def set_motor_pulse_of_circle(self):
        """
        设置电机旋转一圈的所需的脉冲数
        :return:
        """
        pass

    def set_motor_running_current(self):
        """
        设置电机运行电流
        :return:
        """
        pass

    def get_current_absolute_position(self) -> int:
        """
        内部脉冲模式时，当前绝对位置
        0x08		内部脉冲模式时，当前绝对位置低 16 位
        0x09		内部脉冲模式时，当前绝对位置高 16 位
        :return:
        """
        res_list = self.motor.get_single_circle_encoder_pulse_count_value()
        # 低16位整数
        low_bit_int = res_list[0]
        low_bit = f'{low_bit_int:016b}'
        low_bit_str = str(low_bit)
        low_bit_length = len(low_bit_str)
        # 高位补零
        low_zero_pad = "0" * (16 - low_bit_length)
        # 低位二进制字符串
        low_str = low_zero_pad + low_bit_str

        # 高16位整数
        high_bit_int = res_list[1]
        high_bit = f'{high_bit_int:016b}'
        high_bit_str = str(high_bit)
        high_bit_length = len(high_bit_str)
        high_zero_pad = "0" * (16 - high_bit_length)
        # 高位二进制字符串
        high_str = high_zero_pad + high_bit_str
        # 两个16位寄存器读出数据的二进制字符串表示形式
        current_absolute_position = high_str + low_str
        current_absolute = int(current_absolute_position, 2)

        self.logger.info_log(
            f"low bit int: {low_bit_int}--low bit str:{low_str}, high bit int: {high_bit_int}--high bit str:{high_str}, current absolute position: {current_absolute}")

        return current_absolute

    def motor_emergency_stop(self):
        """
        电机急停
        :return:
        """
        self.motor.emergency_stop()

    def point_io_forward_rotate(self, point_acceleration, point_deceleration, rotate_speed):
        """
        电机点动，IO控制，正传+反转
        :return:
        """
        # 获取电机运动状态，0 - 电机停止状态，1- 电机正在运行
        current_motion_speed_status = self.get_motion_status()
        if int(current_motion_speed_status) == 1:
            # 如果当前电机正在运动，则先停止电机
            self.motor.point_io_motor_stop()
            time.sleep(0.5)

        # 选择运动模式，选择0
        self.motor.set_motor_run_mode(run_mode=2)
        # 设置点动模式加速度
        self.motor.set_point_io_acceleration(point_acceleration=point_acceleration)
        # 设置点动模式减速度
        self.motor.set_point_io_deceleration(point_deceleration=point_deceleration)
        # 设置点动模式速度
        self.motor.set_point_io_rotate_speed(rotate_speed=rotate_speed)
        # 开始正向旋转
        self.motor.point_io_forward_rotate()
        # 设置正向旋转
        return 1

    def point_io_back_rotate(self, point_acceleration, point_deceleration, rotate_speed):
        """
        电机点动，IO控制，正传+反转
        :return:
        """
        # 获取电机运动状态，0 - 电机停止状态，1- 电机正在运行
        current_motion_speed_status = self.get_motion_status()
        if int(current_motion_speed_status) == 1:
            # 如果当前电机正在运动，则先停止电机
            # self.motor.point_io_emergency_stop()
            self.motor.point_io_motor_stop()
            time.sleep(0.5)

        # 选择运动模式，选择0
        self.motor.set_motor_run_mode(run_mode=2)
        # 设置点动模式加速度
        self.motor.set_point_io_acceleration(point_acceleration=point_acceleration)
        # 设置点动模式减速度
        self.motor.set_point_io_deceleration(point_deceleration=point_deceleration)
        # 设置点动模式速度
        self.motor.set_point_io_rotate_speed(rotate_speed=rotate_speed)
        # 开始反向旋转
        self.motor.point_io_back_rotate()
        # 测试急停
        # self.motor.point_io_emergency_stop()
        # 设置反向旋转
        return 1

    def point_io_emergency_stop(self):
        """
        IO控制，点动模式下的急停
        :return:
        """
        # 选择运动模式，选择0
        self.motor.set_motor_run_mode(run_mode=0)
        # todo 设置点动模式急停减速度
        # self.motor.set_point_emergency_stop_deceleration(1000)
        # 急停
        self.motor.point_io_emergency_stop()

    def point_decelerate_stop(self):
        """
        点动模式，减速停止
        :return:
        """
        self.motor.point_io_decelerate_stop()

    def fixed_length_reciprocate_motion(self, pulse_count: int = 3000,
                                        max_rotation_speed: int = 50,
                                        acceleration: int = 50,
                                        deceleration: int = 50):
        """
        电机往复运动
        定长运动模式
        :param pulse_count: 行程，脉冲数
        :param max_rotation_speed: 最大旋转速度，单位：RPM，[0,3000]
        :param acceleration: 加速度，单位：r/s^2
        :param deceleration: 减速度，单位：r/s^2
        :return:
        """
        # 设置点位运动参数，最大旋转速度，单位：RPM，[0,3000]
        self.motor.set_max_rotation_speed(max_rotation_speed)
        # 设置加速度
        self.motor.set_acceleration(acceleration)
        # 设置减速度
        self.motor.set_deceleration(deceleration)

        # 选择运动模式，选择0
        self.motor.set_motor_run_mode(run_mode=0)
        # 位置运动方式选择：增量（相对）运动与绝对运动，选择绝对运动
        self.motor.set_position_mode(position_mode=1)

        number: int = 0
        while True:
            number += 1
            self.logger.info_log(f"the {number} reciprocating motion")

            # 清除行程
            self.motor.clear_motor_current_absolute_position()

            current_absolute_position = self.get_current_absolute_position()
            self.logger.info_log(
                f"motor running, after clear motor, current motor absolute position: {current_absolute_position}")

            # 读取下行程寄存器的值
            # current_absolute_position = self.get_current_absolute_position()
            # self.logger.info_log(
            #     f"after clear trip register, current motor absolute position: {current_absolute_position}")

            # 设置运动的行程
            trip_pulse_result = self.motor.set_trip_pulse_count(pulse_count=pulse_count)
            if trip_pulse_result == 0:
                self.logger.error_log("设置行程错误")

            # 18号寄存器选择：1-定长正转
            self.motor.fixed_length_start_motor(rotation_direction=1)
            # 暂停500ms等待寄存器响应
            time.sleep(0.5)

            while True:
                # 获取电机是否跑到位标志
                position_status = self.get_driver_position_complete_status()
                if int(position_status) == 1:
                    self.logger.info_log(f"motor position complete, current position status: {position_status}")
                    break
                current_absolute_position = self.get_current_absolute_position()
                self.logger.info_log(f"motor running, current motor absolute position: {current_absolute_position}")
            # todo 正转完成进行反转
            # 清除行程
            self.motor.clear_motor_current_absolute_position()

            current_absolute_position = self.get_current_absolute_position()
            self.logger.info_log(
                f"reverse motor running, after clear motor, current motor absolute position: {current_absolute_position}")

            # 反向发送负数行程
            trip_pulse_result = self.motor.set_trip_pulse_count(pulse_count=-pulse_count)
            if trip_pulse_result == 0:
                self.logger.error_log("设置行程错误")
            # 18号寄存器选择：2-定长反转
            self.motor.fixed_length_start_motor(rotation_direction=2)
            # 等待寄存器响应
            time.sleep(0.5)
            while True:
                # 获取电机是否跑到位标志
                position_status = self.get_driver_position_complete_status()
                if int(position_status) == 1:
                    self.logger.info_log(f"reverse motor position complete, current position status: {position_status}")
                    break
                current_absolute_position = self.get_current_absolute_position()
                self.logger.info_log(
                    f"reverse motor running, current motor absolute position: {current_absolute_position}")

    def fixed_length_relative_reciprocate_motion(self, pulse_count: int = 3000,
                                                 max_rotation_speed: int = 50,
                                                 acceleration: int = 50,
                                                 deceleration: int = 50):
        """
        电机往复运动
        定长相对运动
        :param pulse_count: 行程，脉冲数
        :param max_rotation_speed: 最大旋转速度，单位：RPM，[0,3000]
        :param acceleration: 加速度，单位：r/s^2
        :param deceleration: 减速度，单位：r/s^2
        :return:
        """
        # 设置点位运动参数，最大旋转速度，单位：RPM，[0,3000]
        self.motor.set_max_rotation_speed(max_rotation_speed)
        # 设置加速度
        self.motor.set_acceleration(acceleration)
        # 设置减速度
        self.motor.set_deceleration(deceleration)

        # 选择运动模式，选择0
        self.motor.set_motor_run_mode(run_mode=0)
        # 位置运动方式选择：增量（相对）运动与绝对运动，选择相对运动
        self.motor.set_position_mode(position_mode=0)
        number: int = 0
        while True:
            number += 1
            self.logger.info_log(f"the {number} reciprocating motion")
            # 清除行程
            self.motor.clear_motor_current_absolute_position()

        pass

    def reciprocate_motion(self, pulse_count: int = 3000,
                           max_rotation_speed: int = 50,
                           acceleration: int = 50,
                           deceleration: int = 50):
        """
        电机往复运动
        :param pulse_count: 行程，脉冲数
        :param max_rotation_speed: 最大旋转速度，单位：RPM，[0,3000]
        :param acceleration: 加速度，单位：r/s^2
        :param deceleration: 减速度，单位：r/s^2
        :return:
        """
        # 选择运动模式，选择3：正传+反转
        # self.motor.set_fixed_length_motion(3)
        # 选择运动模式，选择3：正传+反转
        self.motor.set_motor_run_mode(run_mode=3)
        # 位置运动方式选择：增量（相对）运动与绝对运动，选择绝对运动
        self.motor.set_position_mode(position_mode=1)
        # 清除当前位置
        # self.motor.clear_motor_current_absolute_position()
        # 设置运动的行程
        trip_pulse_result = self.motor.set_trip_pulse_count(pulse_count=pulse_count)
        if trip_pulse_result == 0:
            self.logger.error_log("设置行程错误")
        # 设置点位运动参数，最大旋转速度，单位：RPM，[0,3000]
        self.motor.set_max_rotation_speed(max_rotation_speed)
        # 设置加速度
        self.motor.set_acceleration(acceleration)
        # 设置减速度
        self.motor.set_deceleration(deceleration)
        number: int = 0
        while True:
            number += 1
            self.logger.info_log(f"the {number} reciprocating motion")
            # 开始旋转，7：点动正转/启停；8：点动反转/方向
            self.motor.start_motor(7)

            while True:
                # 获取电机是否跑到位标志
                position_status = self.get_driver_position_complete_status()
                if int(position_status) == 1:
                    self.logger.info_log(f"motor position complete, current position status: {position_status}")
                    break
                current_absolute_position = self.get_current_absolute_position()
                self.logger.info_log(f"current motor absolute position: {current_absolute_position}")

            # 停止旋转
            self.motor.stop_motor()

            while True:
                current_absolute_position = self.get_current_absolute_position()
                self.logger.info_log(
                    f"motor stop, current motor absolute position: {current_absolute_position}")
                # 获取电机运动状态
                motor_motion_status = self.get_motion_status()
                if int(motor_motion_status) == 0:
                    # 如果电机停止，则跳出循环
                    break

            # 设置电机旋转方向，1-定长正转；2-定长反转
            self.motor.start_motor(8)

            while True:
                # 获取电机是否跑到位标志
                position_status = self.get_driver_position_complete_status()
                if int(position_status) == 1:
                    self.logger.info_log(
                        f"reverse run, motor position complete, current position status: {position_status}")
                    break
                current_absolute_position = self.get_current_absolute_position()
                self.logger.info_log(f"reverse run, current motor absolute position: {current_absolute_position}")

            # 停止旋转
            self.motor.stop_motor()
            while True:
                current_absolute_position = self.get_current_absolute_position()
                self.logger.info_log(
                    f"reverse run, motor stop, current motor absolute position: {current_absolute_position}")
                # 获取电机运动状态
                motor_motion_status = self.get_motion_status()
                if int(motor_motion_status) == 0:
                    # 如果电机停止，则跳出循环
                    break


if __name__ == '__main__':
    # num = 40000
    # print(f'{num:016b}')
    motor_action = MotorAction()
    # motor_action.reciprocate_motion(3000, 50, 50, 50)

    motor_action.fixed_length_reciprocate_motion(150000, 50, 50, 50)

    # motor_action.get_zero_status()
    # motor_action.get_motion_status()
    # motor_action.get_driver_alarm_status()
    # motor_action.get_driver_enable_status()
